using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Diagnostics;

namespace GameStateManagementSample.Classes
{
    public sealed class CurveMap<TCurve> where TCurve : BezierSegment
    {
        private readonly float[] _arcLengths;
        private readonly float _ratio;
        public float length { get; private set; }
        public TCurve curve { get; private set; }
        public bool isSet { get { return length!=float.NaN; } }
        public int resolution { get { return _arcLengths.Length; } }

        public CurveMap(int resolution)
        {
            _arcLengths = new float[resolution];
            _ratio = 1f / resolution;
            length = float.NaN;
        }

        public void set(TCurve c)
        {
            curve = c;
            Vector2 o = BezierHelper.GetQuadraticBezierPointAt(0,c.p1,c.p2,c.p3);

            //float ox = o.X;
            //float oy = o.Y;
            float clen = 0;
            int nSamples = _arcLengths.Length;
            for (int i = 0; i < nSamples; i++)
            {
                float t = (i + 1) * _ratio;
                Vector2 p = BezierHelper.GetQuadraticBezierPointAt(t, c.p1, c.p2, c.p3); 
               
                clen += Vector2.Distance(p, o);
                o = p;
                _arcLengths[i] = clen;
            }
            length = clen;
        }

        public Vector2 sample(float u)
        {
            if (u <= 0) return BezierHelper.GetQuadraticBezierPointAt(0,curve);
            if (u >= 1) return BezierHelper.GetQuadraticBezierPointAt(1, curve);

            int index = 0;
            int low = 0;
            int high = resolution - 1;
            float target = u * length;
            float found = float.NaN;

            // Binary search to find largest value <= target
            while (low < high)
            {
                index = (low + high) / 2;
                found = _arcLengths[index];
                if (found < target)
                    low = index + 1;
                else
                    high = index;
            }

            // If the value we found is greater than the target value, retreat
            if (found > target)
                index--;

            if (index < 0) return BezierHelper.GetQuadraticBezierPointAt(0, curve);
            if (index >= resolution - 1) return BezierHelper.GetQuadraticBezierPointAt(1, curve);

            // Linear interpolation for index
            float min = _arcLengths[index];
            float max = _arcLengths[index + 1];
            Debug.Assert(min <= target && max >= target);
            float interp = (target - min) / (max - min);
            Debug.Assert(interp >= 0 && interp <= 1);
            return BezierHelper.GetQuadraticBezierPointAt((index + interp) * _ratio, curve);
        }

        public float map(float u)
        {
            if (u <= 0) return 0;
            if (u >= 1) return 1;

            int index = 0;
            int low = 0;
            int high = resolution - 1;
            float target = u * length;
            float found = float.NaN;

            // Binary search to find largest value <= target
            while (low < high)
            {
                index = (low + high) / 2;
                found = _arcLengths[index];
                if (found < target)
                    low = index + 1;
                else
                    high = index;
            }

            // If the value we found is greater than the target value, retreat
            if (found > target)
                index--;

            if (index < 0) return 0;
            if (index >= resolution - 1) return 1;

            // Linear interpolation for index
            float min = _arcLengths[index];
            float max = _arcLengths[index + 1];
            Debug.Assert(min <= target && max >= target);
            float interp = (target - min) / (max - min);
            Debug.Assert(interp >= 0 && interp <= 1);
            return (index + interp) * _ratio;
        }
    }
}
